#include <ucontext.h>
#include <errno.h>
#include <signal.h>
#include <sys/types.h>
#include <sys/time.h>
#include <unistd.h>
#include <assert.h>
#include <stdlib.h>

#include "gtconst.h"
#include "gtthread.h"

struct itimerval g_time;
struct sigaction act;
static gtthread threads[GTTHREAD_THREADS_MAX];
static gtthread gt_idle_thread;
static long int gtthread_threads_counter = 0;
p_gtthread p_cur_thread;
ucontext_t *p_sched_context;
ucontext_t *p_cur_context;
p_gtthread p_running;
p_gtthread p_waiting;
sigset_t blk_mask, ublk_mask, cur_mask;

void insert_thread_into_running_queue(p_gtthread, p_gtthread);
void remove_thread_from_running_queue(p_gtthread);

p_gtthread gtthread_handle(gtthread_t tid)
{
    if ((int)tid < 0 || (int)tid >= GTTHREAD_THREADS_MAX
            || !threads[(int)tid].used)
        return NULL;
    return (p_gtthread)&threads[(int)tid];
}

void dump_running_queue()
{
    GT_DEBUG_OUT("=======running queue info==============");
    if (p_cur_thread == NULL) {
        GT_DEBUG_OUT("EMPTY");
        GT_DEBUG_OUT("=======================================");
        return;
    }
    p_gtthread pg = p_cur_thread->p_livenext;
    p_gtthread rc = pg;
    do {
        GT_DEBUG_OUT1("Thread %d:", (int)pg);
        GT_DEBUG_OUT1("p_livenext: %d", (int)pg->p_livenext);
        GT_DEBUG_OUT1("p_liveprev: %d", (int)pg->p_liveprev);
        GT_DEBUG_OUT1("active: %d", (int)pg->active);
        GT_DEBUG_OUT1("stack: %d", (int)pg->stack);
        pg = pg->p_livenext;
        //getchar();
    } while (pg != rc);
    GT_DEBUG_OUT("=======================================");
}


static void _gtthread_initialize(void) __attribute__((constructor));

void wakeup_joining_thread(p_gtthread p_th)
{
    p_gtthread p_prev = p_th->p_liveprev;
    // p_gtthread p_next = p_th->p_livenext;
    p_gtthread p_wk = p_th->p_joining;    
    GT_DEBUG_OUT1("[wakeup_thread] waking up thread %d", (int)p_wk->tid);

    GT_DEBUG_OUT1("[wakeup_thread] inserting new thread %d into running queue", (int)p_wk->tid);
    insert_thread_into_running_queue(p_prev, p_wk);
    p_th->p_joining = NULL;
}

void _gtthread_cleanup(p_gtthread p_th)
{
    GT_DEBUG_OUT1("[_gtthread_cleanup] cleaning up thread %d", (int)p_th->tid);

    if (p_th->stack != NULL) // incase main calls gtthread_exit()
        free(p_th->stack);

    GT_DEBUG_OUT1("[_gtthread_cleanup] thread %d's stack is released", p_th->tid);
    // wake up the joining thread and insert them
    if (p_th->p_joining != NULL) {
        GT_DEBUG_OUT("[_gtthread_cleanup] waking up  followers");
        wakeup_joining_thread(p_th);
    }
    remove_thread_from_running_queue(p_th);
    GT_DEBUG_OUT1("[_gtthread_cleanup] thread %d has been removed from running queue", (int)p_th->tid);

#ifdef GT_DEBUG
    dump_running_queue();
#endif
}

void scheduler()
{
    sigprocmask(SIG_BLOCK, &blk_mask, NULL);
    GT_DEBUG_OUT1("[scheduler] current thread is %d", (int)p_cur_thread->tid);
    GT_DEBUG_OUT1("[scheduler] current context is %d", p_cur_context);
    
    if (p_cur_thread->active) {
        GT_DEBUG_OUT1("[scheduler] current thread %d is active, no need to clean up", (int)p_cur_thread->tid);
        p_cur_thread = p_cur_thread->p_livenext;
    }
    // check if there is a canceled thread, clean it up immediately
    while (!p_cur_thread->active) {
        GT_DEBUG_OUT1("[scheduler] thread %d is not active, calling cleanup", (int)p_cur_thread->tid);
        p_cur_thread->cleanup(p_cur_thread);
        if (p_cur_thread == NULL) {// all threads are terminated, exit(0)
            GT_DEBUG_OUT("[scheduler] no running thread, exit(0)");
            exit(0);
        }
        GT_DEBUG_OUT1("[scheduler] finished cleanning up thread %d", p_cur_thread->tid);
        p_cur_thread = p_cur_thread->p_livenext;
    }
    GT_DEBUG_OUT1("[scheduler] next thread selected: %d", (int)p_cur_thread->tid);

    p_cur_context = &p_cur_thread->context;
    GT_DEBUG_OUT1("[scheduler] selected thread is %d", (int)p_cur_thread);
    sigprocmask(SIG_UNBLOCK, &blk_mask, NULL);
    if (setitimer(TIMERTYPE, &g_time, NULL) ) perror("setitimer");
    GT_DEBUG_OUT("[scheduler] Timer set");
    GT_DEBUG_OUT1("[scheduler] switching to the selected thread: %d", p_cur_thread);
    setcontext(p_cur_context);
}


void alarm_handler(int signum, siginfo_t *si, void *old_context)
{
    assert(signum == ALRMTYPE);
    if (!p_cur_thread->active) {
        GT_DEBUG_OUT1("[alarm_handler] Thread %d is not active, setcontext", p_cur_thread);
        setcontext(p_sched_context);
    } else if (p_cur_thread->yield) {
        GT_DEBUG_OUT1("[alarm_handler] Thread %d yields, setcontext", p_cur_thread->tid);
        p_cur_thread->yield = 0;
        setcontext(p_sched_context);
    } else {
        GT_DEBUG_OUT1("[alarm_handler] Thread %d is active, swapcontext", p_cur_thread);
        swapcontext(p_cur_context, p_sched_context);
    }
}

void _gtstart_thread(p_gtthread p_th)
{
    GT_DEBUG_OUT1("[_gtstart_thread] thread %d starts", p_th->tid);
    p_th->p_retval = p_th->start_routine(p_th->args);
    GT_DEBUG_OUT1("[_gtstart_thread] thread %d finished", p_th->tid);
    p_th->active = 0;
    sigprocmask(SIG_UNBLOCK, &blk_mask, NULL);
    while(1); // use up all left quantum, and wait for ALRMTYPE
}


void idle()
{
    GT_DEBUG_OUT("in idle()...................");
    sigprocmask(SIG_UNBLOCK, &blk_mask, NULL);
    while (1);
}

void init_idle()
{
    getcontext(&gt_idle_thread.context);
    gt_idle_thread.stack = (char*)malloc(GT_STACKSIZE);
    gt_idle_thread.context.uc_stack.ss_sp = gt_idle_thread.stack;
    gt_idle_thread.context.uc_stack.ss_size = GT_STACKSIZE;
    gt_idle_thread.context.uc_stack.ss_flags = 0;
    sigemptyset(&gt_idle_thread.context.uc_sigmask);
    makecontext(&gt_idle_thread.context, idle, 0);
}


void init_threads()
{
    int i;
    for (i=0; i<GTTHREAD_THREADS_MAX; ++i) {
        threads[i].p_livenext = threads[i].p_liveprev = 
            threads[i].p_waitnext = threads[i].p_waitprev = NULL;
        threads[i].active = 0;
        threads[i].tid = (gtthread_t)-1;
        threads[i].cleanup = _gtthread_cleanup;
        threads[i].stack = NULL;
        threads[i].start_routine = NULL;
        threads[i].args = NULL;
        threads[i].policy = GT_SCHED_DEFAULT;
        threads[i].priority = GT_PRIO_DEFAULT;
        threads[i].p_joining = NULL;
        threads[i].used = 0;
        threads[i].yield = 0;
    }
}

void init_alarm(long period)
{
    sigemptyset(&blk_mask);
    sigaddset(&blk_mask, ALRMTYPE);

    g_time.it_interval.tv_sec = 0;
    g_time.it_interval.tv_usec = 0;
    g_time.it_value.tv_sec = 0;
    g_time.it_value.tv_usec = period;    

    sigemptyset(&act.sa_mask);
    act.sa_flags = SA_SIGINFO | SA_RESTART;
    act.sa_sigaction = alarm_handler;
    if(sigaction(ALRMTYPE, &act, NULL) != 0)
        perror("sigaction");
}

void init_scheduler()
{
    p_sched_context = &threads[SCHED_THREAD].context;
    threads[SCHED_THREAD].stack = (char*)malloc(GT_STACKSIZE);
    threads[SCHED_THREAD].used = 1;
    getcontext(p_sched_context);
    p_sched_context->uc_stack.ss_sp = threads[SCHED_THREAD].stack;
    p_sched_context->uc_stack.ss_size = GT_STACKSIZE;
    p_sched_context->uc_stack.ss_flags = 0;
    sigemptyset(&p_sched_context->uc_sigmask);
    makecontext(p_sched_context, scheduler, 0);
}

void init_main_thread()
{
    threads[MAIN_THREAD].p_livenext = &threads[MAIN_THREAD];
    threads[MAIN_THREAD].p_liveprev = &threads[MAIN_THREAD];
    threads[MAIN_THREAD].active = 1;
    threads[MAIN_THREAD].tid = (gtthread_t)MAIN_THREAD;
    threads[MAIN_THREAD].cleanup = _gtthread_cleanup;
    threads[MAIN_THREAD].used = 1;
}

void _gtthread_initialize(void)
{
   // Initialize the threads array
    init_threads();
    GT_DEBUG_OUT("[_gtthread_initialize] init_threads() successfully");

    // Initialize the main thread
    init_main_thread();
    GT_DEBUG_OUT("[_gtthread_initialize] init_main_thread() successfully");
    
    // Initialize idle thread
    init_idle();
    GT_DEBUG_OUT("[_gtthread_initialize] init_idle() successfully");
}

void gtthread_init(long period)
{ 
    // Initialize alarm
    init_alarm(period);
    GT_DEBUG_OUT1("init_alarm(%d) successfuly", period);

    // Initialize sigaction
    init_scheduler();
    GT_DEBUG_OUT("init_scheduler() successfully");

    p_cur_thread = &threads[MAIN_THREAD];
    GT_DEBUG_OUT1("[gtthread_init] p_cur_thread is pointed to %d", &threads[MAIN_THREAD]);
    p_cur_context = &threads[MAIN_THREAD].context;
    swapcontext(&threads[MAIN_THREAD].context, &threads[SCHED_THREAD].context);
}

void insert_thread_into_running_queue(p_gtthread p_entry, p_gtthread p_th)
{
    GT_DEBUG_OUT1("[insert_thread_into_running_queue] p_cur_thread: %d", p_cur_thread);
    GT_DEBUG_OUT1("[insert_thread_into_running_queue] p_th: %d", p_th);
    p_th->p_livenext = p_entry->p_livenext;
    p_th->p_liveprev = p_entry;
    GT_DEBUG_OUT1("[insert_thread_into_running_queue] p_entry: ", (int)p_entry);
    GT_DEBUG_OUT1("[insert_thread_into_running_queue] p_th: ", (int)p_th);
    p_entry->p_livenext->p_liveprev = p_th;
    p_entry->p_livenext = p_th;
#ifdef GT_DEBUG
    dump_running_queue();
#endif
}

void remove_thread_from_running_queue(p_gtthread p_th)
{
    GT_DEBUG_OUT1("[remove_thread_from_running_queue] %d", (int)p_th->tid);
    p_gtthread p_prev = p_th->p_liveprev;
    p_gtthread p_next = p_th->p_livenext;
    if (p_prev == p_th && p_th == p_next) {
        GT_DEBUG_OUT1("[remove_thread_from_running_queue] thread %d is the last thread in running queue, process will exit soon", (int)p_th->tid);
        p_cur_thread = NULL;
        return;
    }
//    GT_DEBUG_OUT1("p_th->p_liveprev %d", p_prev);
//    GT_DEBUG_OUT1("p_th->p_livenext %d", p_next);
    // Remove from running queue
    p_prev->p_livenext = p_next;
    p_next->p_liveprev = p_prev;
//    GT_DEBUG_OUT1("p_prev->p_livenext %d", p_prev->p_livenext);
//    GT_DEBUG_OUT1("p_next->p_liveprev %d", p_next->p_liveprev);
#ifdef GT_DEBUG
    dump_running_queue();
#endif
}

int gtthread_create(gtthread_t *thread,
                    void *(*start_routine)(void *),
                    void *arg)
{
    int i, id = -1;
    sigprocmask(SIG_BLOCK, &blk_mask, &cur_mask);
    // start from 2 since 0 is for main and 1 is for scheduler    
    for (i=2; i<GTTHREAD_THREADS_MAX; ++i)
        if (!threads[i].used) {
            threads[i].used = 1;
            id = i;
            break;
        }
    sigprocmask(SIG_UNBLOCK, &blk_mask, NULL);
    if (id == -1) return EAGAIN; // reached max

    GT_DEBUG_OUT1("[gtthread_create] found a free spot %d", id);
    threads[id].stack = (char *)malloc(GT_STACKSIZE);
    if (threads[id].stack == NULL) {
        perror("malloc");
        return EAGAIN; // not enough resource
    }
    getcontext(&threads[id].context);
    threads[id].context.uc_stack.ss_sp = threads[id].stack;
    threads[id].context.uc_stack.ss_size = GT_STACKSIZE;
    threads[id].context.uc_stack.ss_flags = 0;
    sigemptyset(&threads[id].context.uc_sigmask);
    threads[id].start_routine = start_routine;
    threads[id].args = arg;
    threads[id].tid = (gtthread_t)(id);
    
    makecontext(&threads[id].context, _gtstart_thread, 1, &threads[id]);
    GT_DEBUG_OUT1("[gtthread_create] new thread context is %d", (int)&threads[id].context);
    threads[id].active = 1;
    *thread = (gtthread_t)id;
    GT_DEBUG_OUT1("[gtthread_create] new thread id is %d", (int)*thread);

    // Adding the spawned thread into running queue
    GT_DEBUG_OUT("[gtthread_create] before insertion, signal is blocked");
    GT_DEBUG_OUT1("[gtthread_create] p_cur_thread %d", p_cur_thread);
    GT_DEBUG_OUT1("[gtthread_create] threads[id] %d", &threads[id]);

    sigprocmask(SIG_BLOCK, &blk_mask, &cur_mask);
    GT_DEBUG_OUT("[gtthread_create] inserting new thread into queue");
    insert_thread_into_running_queue(p_cur_thread->p_liveprev, (p_gtthread)&threads[id]);
    sigprocmask(SIG_UNBLOCK, &blk_mask, NULL);
    GT_DEBUG_OUT1("[gtthread_create] thread %d is added into running queue", (int)*thread);
    
    return 0;
}

int gtthread_join(gtthread_t thread, void **status)
{
    GT_DEBUG_OUT1("[gtthread_join] joining thread %d", (int)thread);
    int rc;
    gtthread_t self = gtthread_self();
    p_gtthread t_joined = gtthread_handle(thread);

    if (gtthread_equal(thread, self)) 
        return EDEADLK;

    if (!t_joined->used || t_joined->p_joining != NULL) 
        return EINVAL;

    if (thread < 0 || (int)thread >= GTTHREAD_THREADS_MAX) 
        return ESRCH;

    if (!t_joined->active) {
        sigprocmask(SIG_UNBLOCK, &blk_mask, NULL);
        GT_DEBUG_OUT1("[gtthread_join] joined thread %d is not active", (int)t_joined->tid);
        if (status != NULL)
            *status = t_joined->p_retval;
        return 0;
    }

    sigprocmask(SIG_BLOCK, &blk_mask, &cur_mask);
    remove_thread_from_running_queue(p_cur_thread);
    t_joined->p_joining = p_cur_thread;
    GT_DEBUG_OUT1("[gtthread_join] pointed joined thread %d's joining pointer to current thread", (int)t_joined->tid);
    // else if t_joined is still active then this thread should wait
    GT_DEBUG_OUT1("[gtthread_join] current thread %d is suspended", (int)self);
    gtthread_yield();

    sigprocmask(SIG_UNBLOCK, &blk_mask, NULL);
    if (status != NULL) {
        *status = t_joined->p_retval;
    }
    return 0;
}

int gtthread_cancel(gtthread_t thread)
{
    p_gtthread p_handle = gtthread_handle(thread);
    if (p_handle == NULL) return ESRCH;
    GT_DEBUG_OUT1("[gtthread_cancel] thread %d is canceling thread", (int)p_cur_thread->tid);
    GT_DEBUG_OUT1("[gtthread_cancel] %d", (int)p_handle->tid);
    p_handle->active = 0;
    GT_DEBUG_OUT1("[gtthread_cancel] thread %d's active is set to be 0", (int)p_handle->tid);
    return 0;
}

gtthread_t gtthread_self(void)
{
    return p_cur_thread->tid;
}

int gtthread_equal(gtthread_t t1, gtthread_t t2)
{
    return (t1 == t2);
}

void gtthread_yield(void)
{
    GT_DEBUG_OUT1("[gtthread_yield] thread %d is yielding", p_cur_thread->tid);
    sigprocmask(SIG_BLOCK, &blk_mask, &cur_mask);
    p_cur_thread->yield = 1;
    GT_DEBUG_OUT1("[gtthread_yield] thread %d yield flag is set to 1", p_cur_thread->tid);
    swapcontext(p_cur_context, &gt_idle_thread.context);
    GT_DEBUG_OUT1("[gtthread_yield] thread %d returns from yield()", p_cur_thread->tid);
}

void gtthread_exit(void *retval)
{
    GT_DEBUG_OUT1("[gtthread_exit] thread %d is exiting", (int)p_cur_thread->tid);
    p_cur_thread->p_retval = retval;
    p_cur_thread->active = 0;
    GT_DEBUG_OUT1("[gtthread_exit] thread %d's active is 0", (int)p_cur_thread->tid);
    gtthread_yield();
}


